import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.*;

import java.util.ArrayList;

public class Master {
	float distanceToWall;
	double angleToWall;
	private TachoPilot p;
	//private int angle = 45;
	Wallfollow wf;
	static Master m = new Master();
	BarCode lastBarCode;
	Thread t;
	BarCodeFSMThread barcodelezer;
	Driver goldieDriver;
	//SimpleNavigator navigator;
	BTConnection connection;
	DataInputStream dataIn;   //todo: niet nodig?
	DataOutputStream dataOut;
	boolean barcodeDetected;
	int CurrentCode;
	int previousCode;
	Float moveSpeed = 30f;
	boolean trackCompleted = false;
	boolean trackCompletedOld = false;
	boolean phase2=false;
	int currentBarCodeIndex=0;
	int trackSimulationLengthRemaining= 0;
	float expectedAngleToWall=0;
//	boolean fillTrack=false;

	//        int posx = 0;
	//        int posy = 0;
	//        int richting = 0;
	//        int panelNumber = 0;
	//        ArrayList<BarCode> track = new ArrayList<BarCode>();

	TrackDetector trackDetector;



	public static void main(String[] args)
	{
		m.go();
	}

	public Master(){
		p = new TachoPilot(5.52012f, 11.14f, Motor.A, Motor.C,true);
		goldieDriver = new Driver(p,50, moveSpeed);
		t = new Thread(new DistanceThread());
		//bluetooth keuze
		System.out.println("Choose Bluetooth");
		int id = Button.waitForPress();
		if(id == Button.ID_LEFT){
			RConsole.openBluetooth(0);
		}
		else if(id == Button.ID_RIGHT){
			connectBluetooth();
			Thread d = new Thread(new DataThread());
			d.start();
		}
//		else
//			fillTrack=true;

		//gecalibreerde tachopilot

		p.setMoveSpeed(moveSpeed);


		barcodelezer = new BarCodeFSMThread(this);
		Motor.B.setSpeed(900); //max speed
		//plaatsje reserveren op arraylist voor nu nog onbekende eerste barcode

		trackDetector = new TrackDetector(this);
		// Nieuwe muurvolger starten.
	}

	private void go(){
		//begin met rechtdoor rijden tot volgende barcode
		goldieDriver.addCommand(new Command(0, 40, false, CommandEnum.TRAVEL));
		t.start();
		wf = new Wallfollow(this, goldieDriver, false, moveSpeed);
		wf.pause();
		barcodelezer.start();
//		if(fillTrack){
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.LINKS);
//			trackDetector.nextPanel(BarCode.RECHTS);
//			trackDetector.nextPanel(BarCode.RECHTDOOR);
//			trackDetector.nextPanel(BarCode.RECHTS);
//		}
	}


	/**
	 * Stelt in of track volledig gedetecteerd is.
	 * @param trackCompleted
	 */
	public void setTrackCompleted(boolean trackCompleted){
		//System.out.println("track completed: " + trackCompleted);
		this.trackCompleted = trackCompleted;
	}

	private void setSpeed(float speed){
		this.moveSpeed=speed;
		p.setMoveSpeed(speed);
	}

	private void sleep(int milliSeconds){
		try{
			Thread.sleep(milliSeconds);
		}catch(InterruptedException e){
		}
	}

	/**
	 * Called by external class if a barcode is detected.
	 * @param bc
	 */
	public void barCodeDetected (BarCode bc){
		//System.out.println("barcode detected: " + bc.toString());


		processBarcode(bc);
		if (!trackCompleted)
			trackDetector.nextPanel(bc);
	}

	private void processBarcode(BarCode bc){
		System.out.println("Barcode detected: " + bc);
		if(trackCompleted){

			previousCode = CurrentCode;
			if (bc != BarCode.WITTELIJN && bc != BarCode.ZWARTELIJN){
				currentBarCodeIndex++;
				if(!phase2 && bc==BarCode.RECHTDOOR){
					phase2=true;
					setSpeed(30f);
				}
				if(phase2){
					//					if(currentBarCodeIndex==50)
					//						setSpeed(40f);
					//if(currentBarCodeIndex==75)
					//setSpeed(50f);
					executeFase2(bc);
				}
				else
					executeFase1(bc);
			}
		}

		//if (!trackCompleted){
		else{
			executeFase1(bc);
		}
	}

	/**
	 * Code for phase 1
	 * @param barCode
	 */
	private void executeFase1(BarCode barCode) {
		barcodeDetected = true;
		this.lastBarCode = barCode;
		float straightDistance= 15f;
		Command straight = new Command(0, straightDistance, true, CommandEnum.TRAVEL);
		if (barCode == BarCode.RECHTDOOR)
			wf.restart();
		else if (barCode==BarCode.RECHTS){
			wf.pause();
			changeWallSide(true);
			goldieDriver.addCommand(straight);
			goldieDriver.addCommand(new Command(-50, 90, true, CommandEnum.STEER));
			goldieDriver.addCommand(new Command(0, 10, true, CommandEnum.TRAVEL));
			wf.restart();
		}
		else if (barCode==BarCode.LINKS){
			wf.pause();
			changeWallSide(false);
			goldieDriver.addCommand(straight);
			goldieDriver.addCommand(new Command(50, 90, true, CommandEnum.STEER));
			goldieDriver.addCommand(new Command(0, 10, true, CommandEnum.TRAVEL));
			wf.restart();
		}
		else if (barCode==BarCode.WITTELIJN){
			wf.restart();
		}
		else if (barCode==BarCode.ZWARTELIJN){
			wf.restart();
		}
	}

	/**
	 * Code for phase 2
	 * @param bc
	 */
	private void executeFase2(BarCode bc) {

		BarCode expectedBarCode = trackDetector.getBarCodeAt(currentBarCodeIndex);
		//enkel beginnen aan fase 2 op stuk rechtdoor;
		barcodeDetected = true;
		this.lastBarCode = expectedBarCode;
		lastBarCode = expectedBarCode;
		//controleer simulatie niet fout zit
		if(expectedBarCode != bc && expectedBarCode == BarCode.RECHTDOOR && currentBarCodeIndex !=0 && trackDetector.getBarCodeAt(currentBarCodeIndex-1)== BarCode.RECHTDOOR){
			System.out.println("track simulation was broken at barcodeIndex: " + currentBarCodeIndex);    
			trackSimulationBroken(); 
		}else{
			//simulatie juist -> fase 2 algoritme uitvoeren
			if(trackSimulationLengthRemaining<1){
				//System.out.println(currentBarCodeIndex + "current barcode: " + expectedBarCode);
				simulateNewTrackSequence(expectedBarCode);
			}
			else{
				System.out.println("repeating same phase");
				trackSimulationLengthRemaining--;
			}
			//executeFase1(expectedBarCode);
		}
	}


	/**
	 * stelt nieuwe sequentie en bijhorende procedure op voor volgende barcodes
	 * @param bc
	 */
	private void simulateNewTrackSequence(BarCode bc) {
		
		final float distanceToWall=25f;
		float incidentAngle=0f;
		switch (bc){
		case RECHTDOOR:
			setSpeed(30f);
			int cnt = countStraightTracksAfterIndex(currentBarCodeIndex);
			BarCode bcAfterRechtdoor = trackDetector.getBarCodeAt(currentBarCodeIndex + cnt);
			int side=0;
			boolean correctDistance=false;
			if(bcAfterRechtdoor==BarCode.LINKS){
				side = -1;
				if(trackDetector.getBarCodeAt(currentBarCodeIndex-1)==BarCode.RECHTS)
					correctDistance=true;
				System.out.println("RECHTDOOR + LINKS");
				CurrentCode = 0;
			}
			else if(bcAfterRechtdoor == BarCode.RECHTS){
				side = 1;
				if(trackDetector.getBarCodeAt(currentBarCodeIndex-1)==BarCode.LINKS)
					correctDistance=true;
				System.out.println("RECHTDOOR + RECHTS");				
				CurrentCode = 1;
			}
			else
				throw new IllegalStateException("rechts of links barcode verwacht na rechtdoor");
			if (cnt==1){
				float distance = correctDistance? 57:63;
				if(wf.getSide()!=side)
					goldieDriver.addCommand(new Command(0,10, true,CommandEnum.TRAVEL));
				setWallFollow(distance,side);
				trackSimulationLengthRemaining=0;
				expectedAngleToWall=0f;
			}
			else if(cnt>1){
				float distance = 57 - (57-getDistance())/cnt;
				if(wf.getSide()!=side)
					goldieDriver.addCommand(new Command(0,10, true,CommandEnum.TRAVEL));
				setWallFollow(distance, side);
				trackSimulationLengthRemaining=0;
			}
			break;

		case RECHTS:
			setSpeed(30f);
			switch (trackDetector.getBarCodeAt(currentBarCodeIndex+1)){
			case RECHTS:
				setWallFollow(60, 1);
				//int angle = previousCode==4 ? 45: 80; 
				goldieDriver.addCommand(new Command(-60, 80-expectedAngleToWall, true, CommandEnum.STEER));
				wf.restart();
				trackSimulationLengthRemaining=0;
				System.out.println("RECHTS + RECHTS");				
				CurrentCode = 2;
				expectedAngleToWall=0;
				break;
			case RECHTDOOR:
				cnt = countStraightTracksAfterIndex(currentBarCodeIndex+1);
				bcAfterRechtdoor = trackDetector.getBarCodeAt(currentBarCodeIndex+1 + cnt);
				if(bcAfterRechtdoor==BarCode.RECHTS){
					setWallFollow(60, 1);
					goldieDriver.addCommand(new Command(-60,90-expectedAngleToWall, true, CommandEnum.STEER));
				}
				else{
					setWallFollow(60, -1);
					goldieDriver.addCommand(new Command(-60,70-expectedAngleToWall, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(60,20, true, CommandEnum.TRAVEL));
				}
				trackSimulationLengthRemaining=0;
				System.out.println("RECHTS + RECHTDOOR");				
				CurrentCode = 3;
				expectedAngleToWall=0;
				break;
			case LINKS:
				
				switch (trackDetector.getBarCodeAt(currentBarCodeIndex+2)){
				case RECHTS:
					incidentAngle=45f;
					//				if(expectedAngleToWall<20f)
					//					goldieDriver.addCommand(new Command(-60,incidentAngle-10f - expectedAngleToWall, true, CommandEnum.STEER));
					//				else
					goldieDriver.addCommand(new Command(-60,incidentAngle-expectedAngleToWall, true, CommandEnum.STEER));
					if (previousCode ==4)
						goldieDriver.addCommand(new Command(-60,15, true, CommandEnum.STEER));

					goldieDriver.addCommand(new Command(-60,40, true, CommandEnum.TRAVEL));
					//goldieDriver.addCommand(new Command(60,40, true, CommandEnum.STEER));
					setWallFollow(distanceToWall,-1);
					wf.pause();
					goldieDriver.addCommand(new Command(-60,80, false, CommandEnum.TRAVEL));
					trackSimulationLengthRemaining=1;
					System.out.println("RECHTS + LINKS + RECHTS");				
					CurrentCode = 4;
					expectedAngleToWall=incidentAngle;
					break;
				case RECHTDOOR:
					//setWallFollow(60,-1);
					wf.pause();
					incidentAngle = 55f;
					goldieDriver.addCommand(new Command(-60,incidentAngle - expectedAngleToWall, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(-60,15, false, CommandEnum.TRAVEL));
					trackSimulationLengthRemaining=0;
					System.out.println("RECHTS + LINKS + RECHTDOOR");				
					CurrentCode = 5;
					expectedAngleToWall=90-incidentAngle;
					break;
				case LINKS:
					//setWallFollow(60,-1);
					wf.pause();
					incidentAngle=40;
					goldieDriver.addCommand(new Command(-60, incidentAngle, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(60, 40, false, CommandEnum.TRAVEL));
					//goldieDriver.addCommand(new Command(60, 40, true, CommandEnum.STEER));
					trackSimulationLengthRemaining=0;
					System.out.println("RECHTS + LINKS + LINKS");				
					CurrentCode = 6;
					expectedAngleToWall=90-incidentAngle;
					break;
				}
				break;

			}
			break;


		case LINKS:
			setSpeed(30f);
			switch (trackDetector.getBarCodeAt(currentBarCodeIndex+1)){
			case LINKS:
				wf.pause();
				setWallFollow(60, -1);
				//int angle = previousCode==11 ? 45: 80; 
				goldieDriver.addCommand(new Command(60, 80-expectedAngleToWall, true, CommandEnum.STEER));
				wf.restart();
				trackSimulationLengthRemaining=0;
				System.out.println("LINKS + LINKS");
				CurrentCode = 7;
				expectedAngleToWall=0f;
				break;
			case RECHTDOOR:
				wf.pause();
				cnt = countStraightTracksAfterIndex(currentBarCodeIndex+1);
				bcAfterRechtdoor = trackDetector.getBarCodeAt(currentBarCodeIndex+1 + cnt);
				if(bcAfterRechtdoor==BarCode.LINKS){
					setWallFollow(60, -1);
					goldieDriver.addCommand(new Command(60,90-expectedAngleToWall, true, CommandEnum.STEER));
				}
				else{
					setWallFollow(60, 1);
					goldieDriver.addCommand(new Command(60,70-expectedAngleToWall, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(60,15, true, CommandEnum.TRAVEL));
				}
				//setWallFollow(50, -1);
				//goldieDriver.addCommand(new Command(60, 90-expectedAngleToWall, true, CommandEnum.STEER));
				trackSimulationLengthRemaining=0;
				System.out.println("LINKS + RECHTDOOR");
				CurrentCode = 8;
				expectedAngleToWall=0;
				break;
			case RECHTS:
				switch (trackDetector.getBarCodeAt(currentBarCodeIndex+2)){
				case RECHTS:
					//setWallFollow(60,1);
					wf.pause();
					incidentAngle=40;
					goldieDriver.addCommand(new Command(60, incidentAngle, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(60, 40, false, CommandEnum.TRAVEL));
					//goldieDriver.addCommand(new Command(-60, 40, true, CommandEnum.STEER));
					trackSimulationLengthRemaining=0;
					System.out.println("LINKS + RECHTS + RECHTS");
					CurrentCode = 9;
					expectedAngleToWall=90-incidentAngle;
					break;
				case RECHTDOOR:
					wf.pause();
					incidentAngle=55f;
					goldieDriver.addCommand(new Command(60,incidentAngle-expectedAngleToWall, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(-60,30, false, CommandEnum.TRAVEL));
					trackSimulationLengthRemaining=0;
					System.out.println("LINKS + RECHTS + RECHTDOOR");
					CurrentCode = 10;
					expectedAngleToWall=90-incidentAngle;
					break;
				case LINKS:

					incidentAngle = 45f;
					//goldieDriver.addCommand(new Command(60,incidentAngle-expectedAngleToWall, true, CommandEnum.STEER));
					//				if(expectedAngleToWall<20f)
					//					goldieDriver.addCommand(new Command(-60,incidentAngle-10f - expectedAngleToWall, true, CommandEnum.STEER));
					//				else
					//					goldieDriver.addCommand(new Command(-60,incidentAngle-expectedAngleToWall, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(60,incidentAngle-expectedAngleToWall, true, CommandEnum.STEER));

					//				if (previousCode != 11)
					//					goldieDriver.addCommand(new Command(60,50, true, CommandEnum.STEER));
					goldieDriver.addCommand(new Command(0,40, true, CommandEnum.TRAVEL));
					//goldieDriver.addCommand(new Command(-60,40, true, CommandEnum.STEER));
					setWallFollow(distanceToWall,1);
					wf.pause();
					goldieDriver.addCommand(new Command(-60,80, false, CommandEnum.TRAVEL));
					trackSimulationLengthRemaining=1;
					System.out.println("LINKS + RECHTS + LINKS");
					CurrentCode = 11;
					expectedAngleToWall=incidentAngle;
					break;
				}
				break;

			}
			break;
		}



	}

	/**
	 * Returnt aantal segmenten rechtdoor vanaf gegeven barcode incl gegeven barcode.
	 * 
	 * @param currentBarCodeIndex2
	 * @return
	 */
	private int countStraightTracksAfterIndex(int currentBarCodeIndex2) {
		int cnt=0;
		while(trackDetector.getBarCodeAt(currentBarCodeIndex2+cnt)==BarCode.RECHTDOOR){
			cnt++;
			if(cnt>20){
				throw new IllegalStateException();
			}
		}
		return cnt;
	}

	/**
	 * simulatie afbreken en fase 1 hervatten
	 */
	private void trackSimulationBroken() {
		currentBarCodeIndex=0;
		setTrackCompleted(false);
		trackDetector.resetTrack();
		wf.setDistance(37f);
		this.moveSpeed= 30f;
		p.setMoveSpeed(moveSpeed);
		phase2=false;
	}


	public BarCode getBarCode(){
		return this.lastBarCode;
	}


	public boolean lineDetected(){

		return barcodelezer.readingStrip;
	}

	public void driveForward(){
		if(!phase2){
			wf.pause();
			goldieDriver.addCommand(new Command(0, 30, false, CommandEnum.TRAVEL));
		}
	}

	/**
	 * Zet camera naar linkerkant als left == true, anders naar rechts.
	 */
	private void changeWallSide(boolean left){
		//int currentSide = wf.getSideLeft() ? 1:-1;
		//System.out.println("current side:" + currentSide);
		if(wf.getSideLeft()!=left){

			int side = left==true ? 1:-1;
			//System.out.println("desired side: "  + side);
			int turn = (int)(side*(190));
			Motor.B.rotate(turn,true);
			wf.setSideLeft(left);
			//System.out.println(turn);
		}

	}


	private void setWallFollow(float distance, int side){

		boolean left= (side==1);
		//goldieDriver.addCommand(new Command(0, 10, true, CommandEnum.TRAVEL));
		changeWallSide(left);
		wf.setDistance(distance);
		wf.restart();
	}

	public float getDistance(){
		return this.distanceToWall;
	}

	public float getAngleToWall(){
		System.out.println("Angle: "+this.angleToWall);
		return (float)this.angleToWall;
	}

	public float getTravelDistance(){

		return p.getTravelDistance();
	}
	/**
	 * Estabish bluetooth connection to mission control
	 */
	private void connectBluetooth()
	{
		LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		connection = Bluetooth.waitForConnection(); // this method is very patient. 
		LCD.clear();
		LCD.drawString("Connected", 0, 0);
		dataIn = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream();
	}

	class DataThread implements Runnable {
		public void run() {
			while(true){
				try {
					if(barcodeDetected) {
						dataOut.writeInt(-1);
						if(lastBarCode == BarCode.RECHTDOOR)
							dataOut.writeInt(0);
						else if(lastBarCode == BarCode.LINKS)
							dataOut.writeInt(1);
						else if(lastBarCode == BarCode.RECHTS)
							dataOut.writeInt(2);
						else dataOut.writeInt(-1);
						//					dataOut.writeInt(CurrentCode);
						barcodeDetected = false;
					}
					else if(trackCompletedOld!=trackCompleted) {
						dataOut.writeInt(-3);
						if(trackCompleted){
							dataOut.writeInt(0);
						}
						else {
							dataOut.writeInt(1);
						}
						trackCompletedOld=trackCompleted;
					}
					else {
						dataOut.writeInt(-2);
						dataOut.writeFloat(getDistance());
						//					toegevoegd door filip:
						dataOut.writeInt(goldieDriver.getCurrentState());
						dataOut.writeFloat(goldieDriver.getAngleOrSpeed());


					}
					dataOut.flush();
				} catch (IOException e) {

				}
				try {
					Thread.sleep(50);
				} catch (InterruptedException e) {
				}


			}

		}



	}

	class DistanceThread implements Runnable {
		public void run(){
			final UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
			float[] distances;
			distances = new float[10];
			float travelledDistance=getTravelDistance();
			float previousTravelledDistance=0;
			float previousDistanceToWall=0;
			int j = 0;
			while(true){

				for(int i = 9; i>0;i--){
					distances[i]=distances[i-1];
				}

				distances[0] = sonic.getRange(); 

				try{
					Thread.sleep(2);
				}catch(InterruptedException e){
				}

				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				float smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest && distances[i] > 5)
						smallest = distances[i];
				}


				distanceToWall = smallest;
				travelledDistance = getTravelDistance();
				//berekening hoek tov muur
				j++;
				if(travelledDistance !=previousTravelledDistance && j==50){
					j=0;
					angleToWall = Math.asin((previousDistanceToWall-distanceToWall)/(travelledDistance - previousTravelledDistance));
					previousTravelledDistance = travelledDistance;
					previousDistanceToWall=distanceToWall;
				}

			}
		}
	}




}